#include "total_task.h"

float nuc_data[8];
uint8_t ters=0;
//uint8_t nuc_gs=6;
uint8_t run_i=0;

void total_task(void const * argument)
{
		

		uint8_t duoji_ints=1;
    while (1)
    {
		if(duoji_ints==1){
			catcher_close();
			select_middel();
			yaw_middle();
			rocket_front();
			duoji_ints=0;
			rc_ctrl.rc.s[1]=2;
		}
		//350 350 19
				if(ints==2 || ints==1){
					x_s=target_x;
					y_s=target_y;
					a_s=target_angle;
					ints--;
				}
				if(rc_ctrl.rc.ch[3]>450){
                target_x+=0.01;
				dj_ang_set_0+=0.01;
            }
            if(rc_ctrl.rc.ch[3]<-450){
                target_x-=0.01;
				dj_ang_set_0-=0.01;
            }
            if(rc_ctrl.rc.ch[2]>450){
                target_y+=0.01;
            }
            if(rc_ctrl.rc.ch[2]<-450){
                target_y-=0.01;
            }
            if(rc_ctrl.rc.ch[1]<450){
//                if(target_angle>0){
//                    target_angle-=0.01;
//                }
            }
            if(rc_ctrl.rc.ch[1]>-450){
//                if(target_angle<180){
//                    target_angle+=0.01;
//                }
            }
            if(rc_ctrl.rc.s[0]==2){
//							if(run_i==0){
//								moni_nuc++;
//							}
//							run_i=1;
							//catch_plant(1);
							//target_x=450;
//							timescount_flag=1;
//							go_to_plant_flag=1;
							catch_plant(1);
							
							osDelay(1);
							
								//dj_ang_set_0=90;
								//dj_ang_set_1=110;
								//dj_ang_set_2=60;
							//dj_ang_set_3=140;
                
            }
            if(rc_ctrl.rc.s[0]==3){
							//timescount_flag=0;
							//	dj_ang_set_0=75;
            //    dj_ang_set_1=175;
			//					dj_ang_set_2=115;
							//dj_ang_set_3=87;
							
            }
			if(rc_ctrl.rc.s[0]==1){
				//data_get_from_nuc_flag=1;
				/**/
//				target_y=300;
//				osDelay(4500);
//				target_x=100;
//				target_y=100;
//				osDelay(4500);
//				target_x=70;
//				target_y=53;
//				catch_disk(2);
//				osDelay(1);
				
				//catch_disk(1);
				
				//send_flag=1;
				color_set=1;
			}
			//catcher_close();
			start_used=0;
/*
				if(dj_ang_set<=0){
					dj_ang_set=0;
				}
        if(dj_ang_set>=180){
					dj_ang_set=180;
				}
						*/
        
				if(rc_ctrl.rc.ch[4]>0){
							if(run_i==0){
								moni_nuc++;
							}
							run_i=1;
//					key_fla=100;
							//catch_plant(1);
							//target_x=450;
//					catcher_disk();
//					select_red();
				}
				if(rc_ctrl.rc.ch[4]<0){

					//catch_disk(2);
//					catcher_puts();
//				osDelay(80);
//				catcher_close();
					catch_plant(0);
//					catcher_disk();
//					select_yellow();
				}
				else{
					// key_fla=0;
				}
				
				if(go_to_plant_flag==1){
					if(height==0){
						target_x=455;
						target_y=250;
						target_angle=0;
					}
					else if(height==1){
						target_x=455;
						target_y=250;
						target_angle=0;
					}
					else if(height==2){
						target_x=455;
						target_y=250;
						target_angle=0;
					}
				}
				
				if(run_i==1){
            switch(moni_nuc){
								case 1:
//									target_x=450;
//								target_y=200;
//								target_angle=10;
									catch_disk(0);
									break;
								case 2:
//									target_x=470;
//								target_y=250;
//								target_angle=10;
									catch_disk(3);
									break;
								case 3:
//									target_x=470;
//								target_y=220;
//								target_angle=5;
									catch_plant(0);
									break;
								case 4:
									catch_plant(1);
									break;
								case 5:
									catch_piling(0);
									break;
								case 6:
									catch_piling(1);
									break;
								case 7:
									put_things(0);
									break;
								default:
									break;
							}
						run_i=0;
        }
				if(timescount_disk>250 && go_to_disk_flag==1){
					target_y=450;
					target_x=240;
				}
        if(dect_finish==1){
            switch (data_get_from_nuc[0])
            {
                case 0xFF:
                    connect_to_nuc();
                    break;
								case 0x01:
                    catch_disk(data_get_from_nuc[1]);
                    //nuc_gs=data_get_from_nuc[4];
										break;
                case 0x02:
                    catch_plant(data_get_from_nuc[1]);
                    break;
                case 0x03:
                    catch_piling(data_get_from_nuc[1]);
                    break;
								case 0x04 :
										put_things(data_get_from_nuc[1]);
										break;
								case 0x05:
										fuck_you();
										break;
                default:
                    break;
            }
						//cmd_running_if();
        }
				te[0]=(float)data_get_from_nuc[0];
				te[1]=(float)data_get_from_nuc[1];
				te[2]=(float)(send_flag);
				//te[3]=(float)(data_get_from_nuc[4]);
        osDelay(1);
					//motor_read();
    }
    
}

void dect_on_pos(){
	while(abs(DM_target_pos-DM_motor_data.p_int)>=100){
		osDelay(1);
	}
}



void connect_to_nuc(void)
{
    // uint8_t i;
    // send_id=0xFF;
    // for(i=0;i<59;i++){
    //     send_Dats[i]=0x00;
    // }
    // send_Dats[0]=0x21;
    // send_flag=1;
    // dect_finish=0;
    // led_c=2;
    nuc_connect_flag=1;
	led_c=2;
}

void nuc_contrl_der_arm(void)
{
    NUC_DATA_SWICH();
    target_der_x=nuc_data[0];
    target_der_y=nuc_data[1];
    dect_finish=0;
    //for(uint8_t i=0;i<59;i++){
    //    send_Dats[i]=data_get_from_nuc[i+4];
    //}
    //send_id=0x72;
    //send_flag=1;
}

void time_delay(int mills)
{
	osDelay(mills);
}



void catch_disk(uint8_t flag)
{
    //400  350
		arms_used=1;
        if(flag==0x00){
			if(get_color_init==0){
				color_set=data_get_from_nuc[2];
				get_color_init=1;
			}
            nuc_run_if=1;
            target_y=280;//350
            target_angle=15;
            osDelay(1000);
					target_x=350;
					osDelay(2000);
            //dect_on_pos();
					//dect_on_pos();
					//target_y=240;//320
					target_x=440;
					target_angle=0;
					yaw_ping();
					//target_x=390;
					target_y=230;
			target_angle=0;
			osDelay(1000);
					if(color_set==0){//0:red
						target_x=435;
						//407
						target_y=205;
					}
					else if(color_set==1){
						target_x=398;
						;
						target_y=210;
					}
					osDelay(2500);
					//blue 390,205;
					//red 437 205
					//go_to_disk_flag=1;

            
        }
        else if(flag==0x01){//red or blue
            //yaw_ping();
            catcher_puts();
						if(select_flag==1){
							osDelay(100);
							select_red();
							select_flag=0;
							catcher_close();
						}
						else{
							select_red();
						}
            
						if(select_flag==1){
							catcher_close();
							osDelay(35);
							select_flag=0;
						}
						else{
							catcher_close();
							osDelay(135);
						}
            
            //catcher_close();
						//data_get_from_nuc_flag=0;
						select_flag=1;
							//osDelay(1000);
					//select_middel();
        }
        else if(flag==0x02){//yellow
            //yaw_ping();
						
            catcher_puts();
            if(select_flag==1){
							osDelay(100);
							select_yellow();
							select_flag=0;
							catcher_close();
						}
						else{
							select_yellow();
						}
            
						if(select_flag==1){
							catcher_close();
							osDelay(35);
							select_flag=0;
						}
						else{
							osDelay(135);
							catcher_close();
						}
						//data_get_from_nuc_flag=0;
            
					select_flag=1;
					//osDelay(1000);
            //select_middel();
        }
        else if(flag==0x03){//end
					// timescount_flag=1;
					go_to_plant_flag=1;
						target_x=400;
            osDelay(3000);
            target_x=200;
						osDelay(2000);
            target_y=250;
						target_angle=0;
            osDelay(2000);
						target_x=470;
					target_y=250;
//					target_y=270;
//					target_x=455;
					target_angle=10;
//            yaw_yang();
					osDelay(1000);
					// sed_ok();
					//go_to_plant_flag=1;
					beilv=150;
					go_to_disk_flag=0;

        }
		arms_used=0;
        
}

void sed_ok()
{
    send_Dats[0]=sed_count;
//     ***     ***
//     * *     * *
//
//           *
//
//      *         *
//       *       *
//         ******
	sed_count++;
	send_flag=1;
}

void catch_plant(uint8_t flag)
{
		float ii=0.0;
     int i=0;
// //		if(pilling_count>=2){
// //			target_y=400;
// //			target_x=200;
// //			osDelay(2500);
// //			target_y=200;
// //			sed_ok();
// //		}			
// 		beilv=650;
// 	timescount_flag=0;
// 	go_to_plant_flag=0;
    if(flag==0x00){//cube
		plant_run_flag=1;
		beilv=650;
		target_angle=15;
		target_y=275;
			osDelay(2000);
			target_x=420;
			osDelay(1500);
			target_angle=5;
		target_x=400;
		catcher_open();
			osDelay(2000);
		while(plant_run_flag){
			target_y-=0.03;
			if(target_y<130){
				break;
			}
			if(abs(der_a-der_b)<5){
				if(der_b<25){
					i++;
					if(i==15){
						target_y+=10;
						plant_run_flag=0;
					}
					target_y+=15;
					plant_run_flag=0;
				}
				else{
					i=0;
				}
			}

//			if(der_b<100){
//				i=1;
//			}

			osDelay(1);
		}
		osDelay(2000);
		catcher_close();
		yaw_yang();
		osDelay(500);
		target_y=250;
		osDelay(1500);
		target_x=250;
		osDelay(1500);
		target_x=210;
		target_y=195;
		target_angle=0;
		osDelay(3000);
		catcher_open();
		osDelay(500);
		catcher_close();
		target_y=250;
		osDelay(2000);
		target_x=450;
		target_angle=0;
		osDelay(2000);
		sed_ok();
		yaw_ping();

    }
    else if(flag==0x01){//circle
			plant_run_flag=1;
		beilv=650;
		target_angle=15;
		target_y=275;
			osDelay(1000);
			target_x=420;
			osDelay(1000);
			target_angle=5;
		target_x=400;
		catcher_open();
			osDelay(2000);
		while(plant_run_flag){
			target_y-=0.02;
			if(target_y<130){
				break;
			}
			if(target_y<97){
				target_y+=10;
						plant_run_flag=0;
			}
			if(abs(der_a-der_b)<25){
				if(der_b<25 && der_b>7){
					i++;
					if(i==35){
						target_y+=10;
						plant_run_flag=0;
					}
				}
				else{
					i=0;
				}
			}

//			if(der_b<100){
//				i=1;
//			}

			osDelay(1);
		}
		osDelay(2000);
		catcher_close();
		 			yaw_ping();
 			osDelay(500);
 			rocket_plant();
 			target_y=300;
 			osDelay(1500);
 			target_x=350;
 			osDelay(1500);
 			target_x=150;
 			target_y=200;
 			osDelay(1300);
 			target_x=100;
 			target_y=160;
 			osDelay(2000);
 			target_y=140;
 			target_angle=-3;
 			osDelay(2000);
 			catcher_open();
 			osDelay(500);
 			target_y=250;
 			osDelay(1000);
 			rocket_front();
 			target_x=200;
 			target_angle=10;
 			//osDelay(2000);
 			catcher_close();
 			target_y=350;
 			osDelay(1500);
 			target_x=400;
 			target_y=300;
 			osDelay(2000);
 			target_x=470;
 			target_y=250;
			sed_ok();
    }
    else if(flag==0x02){//QR code move
        for(ii=0;ii>-10;ii-=0.05){
					target_angle=ii;
					osDelay(10);
				}
				for(ii=-5;ii<10;ii+=0.05){
					target_angle=ii;
					osDelay(10);
				}
				//sed_ok();
        //i=0;
		//send_id=0x74;
		//for(i=0;i<59;i++){
        //    send_Dats[i]=0x00;
        //}
		///send_flag=1;
			//sed_ok();
    }
    else if(flag==0x03){//return format
//        target_y=250
				;
//			target_x=455;
			osDelay(100);
        sed_ok();
				
    }
    else if(flag==0x04){//catch QR code
        dect_finish=1;
		data_get_from_nuc[2]=0x73;
		data_get_from_nuc[4]=0x00;
    }
		// timescount_flag=1;
		// go_to_plant_flag=1;
		// beilv=150;
}

void catch_piling(uint8_t flag)
{
		arms_used=1;
	beilv=650;
    if(flag==0x00){
        //target_angle=14;
        //1:y=300 2:x=370 3:x=350,y=250 4:dj_ang_set=85 5:dj_ang_set=93
			beilv=650;
			go_to_plant_flag=0;
        nuc_run_if=3;
			yaw_ping();
        target_y=300;
        //
		//dect_on_pos();
        target_x=300;
			osDelay(3000);
			target_y=260;
			target_x=420;
			catcher_open();
			osDelay(3000);
    }
    else if(flag==0x01){
        nuc_run_if=4;
        catcher_open();
        target_y=190;
			osDelay(1500);
			catcher_close();
			osDelay(500);
			target_y=300;
			target_x=200;
			osDelay(2000);
			target_y=230;
			osDelay(1000);
			catcher_open();
			select_red();
			osDelay(600);
			catcher_close();
        sed_ok();
    }
    else if(flag==0x02){
        target_y=240;
		//dect_on_pos();
		time_delay(3000);
		catcher_open();
		select_red();
		//time_delay(500);
		//catcher_open();
		//dect_on_pos();
		time_delay(1000);
		select_middel();
		catcher_close();
		target_y=300;
		//dect_on_pos();
		osDelay(5000);
    }
    else if(flag==0x03){//end;
			
    }
		arms_used=0;
	
}

void put_things(uint8_t flag)
{
		if(flag==0x00){
			nuc_run_if=5;
			yaw_ping();
			osDelay(600);
			rocket_catch();
			osDelay(600);
			target_x=103;
			osDelay(2000);
			target_y=200;
			
			target_angle=0;
			catcher_puts();
			target_y=85;
			osDelay(3000);
			catcher_close();
			osDelay(500);
			target_y=250;
			osDelay(2000);
			target_x=332;
			target_angle=5;
			osDelay(2500);
			target_y=220;
			osDelay(1500);
			catcher_open();
			rocket_front();
			osDelay(500);
			target_y=250;
			osDelay(1500);
			catcher_close();
			target_x=100;
			target_angle=0;
			osDelay(1500);
			target_y=60;
			target_x=60;
			osDelay(1500);
			//motor_stop();
			sed_ok();
			go_to_plant_flag=0;
			rc_ctrl.rc.s[1]=2;
			
			
		}
}

void fuck_you()
{
	target_y=300;
	osDelay(1000);
	target_x=100;
	target_y=100;
	osDelay(3000);
	rc_ctrl.rc.s[1]=2;
}

